/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author Eric
 */
public class MecanumDrive {
    
    Joystick joy1 = new Joystick(1);
    Joystick joy2 = new Joystick(2);
    static Jaguar frontleft;
    static Jaguar frontright;               
    static Jaguar backleft;
    static Jaguar backright;
    
    ADXL345_I2C accel;
    RobotDrive drive;
    Gyro drivegyro;
    Timer drivetimer;
    
    boolean fieldframeinuse=false;
    int mode;
    boolean triggered,autodrivetriggered;
    static int autodrivetype=0;
    
    public MecanumDrive(int fleft,int rleft,int fright,int rright,int robogyro)
    {
        frontleft = new Jaguar(fleft);
        frontright = new Jaguar(fright);
        backleft = new Jaguar(rleft);
        backright = new Jaguar(rright);
    
        accel = new ADXL345_I2C(1,ADXL345_I2C.DataFormat_Range.k8G);
        drive = new RobotDrive(frontleft,backleft,frontright,backright);
        drivegyro=new Gyro(robogyro);
        drivetimer = new Timer();
        mode=1;
        triggered=false;
        autodrivetriggered=false;
      
    }
    
    void resetref()
    {
        drivegyro.reset();
    }
    
    void resetgyroframe()
    {
        if(joy2.getRawButton(11))
        {
            resetref();
        }
    };
    
    void driveFieldFrame()
    {
        fieldframeinuse=true;
        drive.mecanumDrive_Cartesian(-1*joy1.getX(),joy1.getY(),joy2.getX(),360-drivegyro.getAngle());
    }
    
    void driveBodyFrame()
    {
        fieldframeinuse=false;
        drive.mecanumDrive_Polar(joy1.getY(),-1*joy1.getX(),joy2.getX());
    }
    void driveAutoFrame()
    {
        double distanceinput=0;
        double timeinput=0;
        
        
        try
        {
            distanceinput=SmartDashboard.getNumber("Drive Straight Dist:");
            timeinput=SmartDashboard.getNumber("Drive Straight Time:");
        }
        catch(Exception e)
        {}
 
        if(!autodrivetriggered)
        {
            if(joy1.getRawButton(1)||joy1.getRawButton(2)||joy1.getRawButton(3)||joy1.getRawButton(4))
            {
                autodrivetriggered=true;
                drivetimer.stop();
                drivetimer.reset();
                drivetimer.start();
            }
            if(joy1.getRawButton(1))
            {
                DriveEncoder.resetEncoders();
                autodrivetype=1;
            }
       
        }
                
        if(autodrivetriggered)
        {
            if(autodrivetype==1&&driveStraightDist(distanceinput))
            {
                autodrivetriggered=false;
                autodrivetype=0;
            }
        }
        
        
            
    }
    
    void driveMode()
    {
        if(joy2.getTrigger() && !triggered) //runs if trigger pressed for first time
        {
            mode++;
            triggered = true;
            if(mode>= 4)//if the mode exceeds maximum wanted options, sets to one
            {
              mode = 1;  
            }//end if
            if(mode== 1)//set to field drive
            {
                this.driveFieldFrame();
            }//end if
            else if (mode == 2)//set to body drive
            {
                this.driveBodyFrame();
            }//end else if
            else if (mode == 3)
            {
                this.driveAutoFrame();
            }//end else if
        }//end if
        if(!joy2.getTrigger())
        {
            triggered = false;
        }//end if
        
    }//end driveMode
    

    
    void setSensitivity(double factor)
    {
        frontleft.set(frontleft.get()*factor);
        frontright.set(frontright.get()*factor);
        backleft.set(backleft.get()*factor);
        backright.set(backright.get()*factor);
    }
    
    boolean driveStraightDist(double distance)
    {
       if(DriveEncoder.getLeftDistance()<distance)
       {
           drive.mecanumDrive_Polar(1,0,0);
           return false;
       }
       else
       {
           drive.mecanumDrive_Polar(1,0,0);
           return true; //True means I'm done
       }
       
    }
    boolean autoAnyDirectionTime(int sec, int power, int direction) //drive robot in specified direction for specified time
    {
        /*drivetimer.start();
        drive.mecanumDrive_Polar(power, direction, 0);
        if (time.get()>sec)//stops robot at end of set time
        {
            drive.mecanumDrive_Polar(0, 0, 0);
        }//end if
        * */
        return false;
    }//end method autoAnyDirection
    
     public void turnDistance(int distance, int turnDegree)
    {
      /* DriveEncoder myEncoder = new DriveEncoder();
       myEncoder.startEncoder();
       drive.mecanumDrive_Polar(1, 0, 0);
       if(myEncoder.getDistance() >= distance)
       {
           myEncoder.stopEncoder();
           drive.mecanumDrive_Polar(0, 0, turnDegree);
       }*/
       
    }
    
    void driverStationOutputs()
    {
        String drivemode,driveauto;
        
        SmartDashboard.putBoolean("Field Frame:", fieldframeinuse);
        SmartDashboard.putNumber("Gyro Angle", drivegyro.getAngle());
        
        SmartDashboard.putNumber("Front Left",frontleft.get());
        SmartDashboard.putNumber("Front Right",frontright.get());
        SmartDashboard.putNumber("Back Left",backleft.get());
        SmartDashboard.putNumber("Back Right",backright.get());
        
        switch(mode)
        {
            case 1:drivemode="Field Frame";
                break;
            case 2:drivemode="Body Frame";
                break;
            case 3:drivemode="Auto Drive";
                break;
            default:drivemode="Undefined";
                break;
        }
        SmartDashboard.putString("Drive Mode", drivemode);
        
        switch(autodrivetype)
        {
            case 0:driveauto="Disabled";
                break;
            case 1:driveauto="Drive Straight";
                break;
            case 2:driveauto="Turn";
                break;
            case 3:driveauto="Strafe";
                break;
            default:driveauto="Unknown";
                break;
        }
        SmartDashboard.putString("Auto Drive Type", driveauto);
    }
}
